#include <rclcpp/rclcpp.hpp>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>
#include <thread>
#include <atomic>
#include "unitree_api/msg/request.hpp"
#include "common/ros2_sport_client.h"

// 定义要通过网络接收的数据结构（来自ROS1）
struct CtrlData {
    float vx;   // ctrl_cmd_x_linear
    float vy;   // ctrl_cmd_y_linear  
    float vyaw; // ctrl_cmd_z_angular
};

class CtrlBridgeNode : public rclcpp::Node
{
public:
    CtrlBridgeNode() : Node("ctrl_bridge_node"), running_(true)
    {
        // 默认设置：监听端口
        this->declare_parameter("listen_port", 9877);
        
        listen_port_ = this->get_parameter("listen_port").as_int();
        
        // 创建运动控制发布器
        req_puber_ = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 10);
        
        // 创建UDP套接字
        if ((sock_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "UDP套接字创建失败");
            return;
        }
        
        // 设置监听地址
        memset(&server_addr_, 0, sizeof(server_addr_));
        server_addr_.sin_family = AF_INET;
        server_addr_.sin_port = htons(listen_port_);
        server_addr_.sin_addr.s_addr = INADDR_ANY;
        
        // 绑定套接字
        if (bind(sock_fd_, (struct sockaddr*)&server_addr_, sizeof(server_addr_)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "套接字绑定失败");
            close(sock_fd_);
            return;
        }
        
        RCLCPP_INFO(this->get_logger(), "控制桥接节点已启动，监听端口 %d", listen_port_);
        
        // 启动接收线程
        recv_thread_ = std::thread(&CtrlBridgeNode::receiveLoop, this);
    }
    
    ~CtrlBridgeNode()
    {
        running_ = false;
        if (recv_thread_.joinable()) {
            recv_thread_.join();
        }
        if (sock_fd_ >= 0) {
            close(sock_fd_);
        }
    }

private:
    void receiveLoop()
    {
        CtrlData ctrl_data;
        struct sockaddr_in client_addr;
        socklen_t client_len = sizeof(client_addr);
        
        while (running_) {
            // 设置接收超时
            fd_set readfds;
            struct timeval timeout;
            FD_ZERO(&readfds);
            FD_SET(sock_fd_, &readfds);
            timeout.tv_sec = 1;
            timeout.tv_usec = 0;
            
            int result = select(sock_fd_ + 1, &readfds, NULL, NULL, &timeout);
            if (result > 0 && FD_ISSET(sock_fd_, &readfds)) {
                ssize_t recv_len = recvfrom(sock_fd_, &ctrl_data, sizeof(ctrl_data), 0,
                                          (struct sockaddr*)&client_addr, &client_len);
                
                if (recv_len == sizeof(ctrl_data)) {
                    RCLCPP_INFO(this->get_logger(), 
                               "收到控制指令: vx=%.3f, vy=%.3f, vyaw=%.3f", 
                               ctrl_data.vx, ctrl_data.vy, ctrl_data.vyaw);
                    
                    // 发送运动控制指令
                    sendMoveCommand(ctrl_data.vx, ctrl_data.vy, ctrl_data.vyaw);
                }
            }
        }
    }
    
    void sendMoveCommand(float vx, float vy, float vyaw)
    {
        // 平滑减速参数
        constexpr float scale = 1.0f;
        constexpr int steps = 10; // 插值步数
        constexpr int sleep_ms = 25; // 每步间隔ms

        // 如果上一次和本次都是000，直接跳过
        if (vx == 0.0f && vy == 0.0f && vyaw == 0.0f &&
            last_vx_ == 0.0f && last_vy_ == 0.0f && last_vyaw_ == 0.0f) {
            return;
        }

        if (vx == 0.0f && vy == 0.0f && vyaw == 0.0f) {
            // 只有上一次速度较大时才插值减速，否则直接StopMove
            float last_vx = last_vx_;
            float last_vy = last_vy_;
            float last_vyaw = last_vyaw_;
            if (last_vx > 0.3f || last_vy > 0.3f || last_vyaw > 0.2f) {
                for (int i = 1; i <= steps; ++i) {
                    float ratio = float(steps - i) / steps;
                    float vx_interp = last_vx * ratio * scale;
                    float vy_interp = last_vy * ratio * scale;
                    float vyaw_interp = last_vyaw * ratio * scale;
                    unitree_api::msg::Request req;
                    sport_client_.Move(req, vx_interp, vy_interp, vyaw_interp);
                    req_puber_->publish(req);
                    RCLCPP_INFO(this->get_logger(), "插值减速: vx=%.3f, vy=%.3f, vyaw=%.3f", vx_interp, vy_interp, vyaw_interp);
                    std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
                }
                unitree_api::msg::Request req;
                sport_client_.StopMove(req);
                req_puber_->publish(req);
                RCLCPP_INFO(this->get_logger(), "收到000，已平滑减速并发送停止运动指令");
            } else {
                unitree_api::msg::Request req;
                sport_client_.StopMove(req);
                req_puber_->publish(req);
                RCLCPP_INFO(this->get_logger(), "收到000，直接发送停止运动指令");
            }
        } else {
            // 正常Move
            float vx_scaled = vx * scale;
            float vy_scaled = vy * scale;
            float vyaw_scaled = vyaw * scale;
            unitree_api::msg::Request req;
            sport_client_.Move(req, vx_scaled, vy_scaled, vyaw_scaled);
            req_puber_->publish(req);
            RCLCPP_INFO(this->get_logger(), "发送运动指令给狗: vx=%.3f, vy=%.3f, vyaw=%.3f", 
                       vx_scaled, vy_scaled, vyaw_scaled);
        }
        // 记录本次速度
        last_vx_ = vx;
        last_vy_ = vy;
        last_vyaw_ = vyaw;
    }

    // 上一次速度
    float last_vx_ = 0.0f;
    float last_vy_ = 0.0f;
    float last_vyaw_ = 0.0f;

    int sock_fd_ = -1;
    struct sockaddr_in server_addr_;
    int listen_port_;
    std::thread recv_thread_;
    std::atomic<bool> running_;

    // 运动控制相关
    rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr req_puber_;
    SportClient sport_client_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CtrlBridgeNode>());
    rclcpp::shutdown();
    return 0;
}